//
// Created by augtons on 2022/1/21.
//

#include "imu_windswing.h"

_Noreturn void task_windswing_imu(void *args);

Imu imu = {
    .kp = 600.0f,
    .ki = 30.0f,
    .lastTime = 0,
};

void initWindSwingIMU(){
    esp_i2c_install_driver();
    int err = mpu_init_without_int();
    ESP_ERROR_CHECK(err);
    mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);

    /* Push both gyro and accel data into the FIFO. */
    //mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
    mpu_set_sample_rate(50);
    mpu_set_gyro_fsr(500);

    xTaskCreate(task_windswing_imu, "windswing_imu", IMU_TASK_S, NULL, IMU_TASK_P, NULL);
}

_Noreturn void task_windswing_imu(void *args){
    short accel[3];
    short gyro[3];

    unsigned long sensor_timestamp;

    Vector3 vector3accel = {0};
    Vector3 vector3gyro = {0};

    uint8_t update_count = 0;

    float tileAngle = 0;
    float azimuth = 0;

    EulerAngle eulerAngle;

    while (1){
        mpu_get_accel_reg(accel, &sensor_timestamp);
        mpu_get_gyro_reg(gyro, &sensor_timestamp);

        vector3accel.x = (float)accel[0];
        vector3accel.y = (float)accel[1];
        vector3accel.z = (float)accel[2];

        vector3gyro.x = (float)gyro[0];
        vector3gyro.y = (float)gyro[1];
        vector3gyro.z = (float)gyro[2];

        vector3accel.x /= 16384;
        vector3accel.y /= 16384;
        vector3accel.z /= 16384;

        vector3gyro.x /= 131;
        vector3gyro.y /= 131;
        vector3gyro.z /= 131;

//        printf("%f %f %f", vector3accel.x, vector3accel.y, vector3accel.z);
        IMU_Update(&imu, &vector3accel, &vector3gyro);
        if(++update_count > 10){
            IMU_GetTiltAngle(&imu.quaternion, &tileAngle, &azimuth);
            IMU_GetEulerAngle(&imu.quaternion, &eulerAngle);
            set_tileAngle(tileAngle);
            set_azimuth(azimuth);
            set_eularAngle(&eulerAngle);

        }
    }
}